#!/usr/bin/env python3

import rospy
import numpy as np
from sensor_msgs.msg import PointCloud2, PointField
import sensor_msgs.point_cloud2 as pc2
import struct

def create_test_point_cloud():
    """Create a simple test point cloud (a small cube)"""
    points = []
    # Create a 5x5x5 cube of points centered at (0,0,0)
    for x in np.linspace(-2.5, 2.5, 5):
        for y in np.linspace(-2.5, 2.5, 5):
            for z in np.linspace(-2.5, 2.5, 5):
                points.append([x, y, z])
    
    return points

def publish_point_cloud():
    rospy.init_node('test_pointcloud_publisher', anonymous=True)
    pub = rospy.Publisher('/points_raw', PointCloud2, queue_size=1)
    rate = rospy.Rate(1)  # 1 Hz
    
    # Define the point cloud fields
    fields = [
        PointField('x', 0, PointField.FLOAT32, 1),
        PointField('y', 4, PointField.FLOAT32, 1),
        PointField('z', 8, PointField.FLOAT32, 1),
    ]
    
    # Create the test point cloud
    points = create_test_point_cloud()
    
    while not rospy.is_shutdown():
        # Create the point cloud message
        header = rospy.Header()
        header.stamp = rospy.Time.now()
        header.frame_id = "velodyne"
        
        # Create the point cloud
        pc = pc2.create_cloud(header, fields, points)
        
        # Publish the point cloud
        pub.publish(pc)
        rospy.loginfo("Published test point cloud with %d points", len(points))
        
        rate.sleep()

if __name__ == '__main__':
    try:
        publish_point_cloud()
    except rospy.ROSInterruptException:
        pass 